from ctypes import alignment
import sys
import math
import threading

import matplotlib.pyplot as plt

from test_init import *

# system state enum
TO_SELECT=0
NEXT = 1
END = 3

# system set
dir_prefix = out_dir+"/run_data/"
f_out = open(out_dir+"/detail.txt","w")
# sys.stdout = f_out
data_file = open(dir_prefix+"action_long_time.csv","r",newline='')
data = data_file.read().splitlines()

line_num = 0
line_group_size = 5
sensor_info_group = []

title="Grd_Map:"

# system initial
system_state = NEXT
grd_pos_tree = PositionCandTree(10)
sim_pos_tree = PositionCandTree(10)

def read_next_line():
    global line_num,data,grd_pos_tree
    if line_num>=len(data): return False

    line_str = data[line_num]
    line_num+=1

    line = line_str.split(",")
    # print(line_str)
    
    data_type = line[0]
    legal_data = True
    is_sensor_data = False
    if data_type == "SYSTEM_TYPE":
        grd_pos_tree.set_state(line)
    elif data_type == "SENSOR_TYPE":
        is_sensor_data = True
        for i in range(1,5):
            try:
                p = int(line[i])
            except:
                legal_data=False
    elif data_type == "ACTION_TYPE":
        grd_pos_tree.set_action(line[1])
        global title,grd_ax
        title+=line[1]
        grd_ax.set_title(title)
    elif data_type == "ANGLE_TYPE":
        try:
            angle = float(line[1])
            if (grd_pos_tree.is_initialized):
                grd_pos_tree.update_ideal_angle(angle)
        except:
            legal_data=False
    if (legal_data and is_sensor_data):
        d = []
        for i in range(1,5):
            d.append(int(line[i]))
        sensor_info_group.append(d)
    return True

def get_next_data(msg:list):
    global line_num,line_group_size,sensor_info_group,grd_pos_tree

    start_line_num = line_num
    while(read_next_line()):
        # print(len(sensor_info_group))
        if not grd_pos_tree.is_initialized  and len(sensor_info_group)==1:
            break
        if (len(sensor_info_group)>=line_group_size):
            break
    
    end_line_num = line_num
    if (len(sensor_info_group)==0):
        msg.append("file eof")
        return "EOF"


    msg.append("line range = [{},{})".format(start_line_num,end_line_num))
    print("line range = [{},{})".format(start_line_num,end_line_num))

    # print(sensor_info_group)

    ret_rec = [0,0,0,0]
    for ss in sensor_info_group:
        for i in range(4):
            ret_rec[i] += ss[i]

    for i in range(4):
        ret_rec[i]/=len(sensor_info_group)
    
    sensor_info_group.clear()
    
    return ret_rec

def do_once():
    global grd_pos_tree
    msg = []
    line = get_next_data(msg)
    grd_pos_tree.advance_once(line,msg)

    global pos_txt,act_txt,sys_txt

    if (not txt_show): msg.clear()
    pos_txt.set_text("\n".join(msg))
   
    act_msg = grd_pos_tree.action.action_state_str if grd_pos_tree.action else "EMPTY"
    act_txt.set_text(act_msg)

    sys_msg = []
    sys_msg.append(grd_pos_tree.state)
    grd_pos_tree.get_action_state(sys_msg)
    sys_msg = ["{}".format(x) for x in sys_msg]
    sys_txt.set_text("\n".join(sys_msg))
    

def on_press(event):
    print('press', event.key)
    
    if (event.key=='R' or event.key=='r'):
        grd_pos_tree.set_state("SYSTEM_TYPE,RESET")
    elif (event.key=='B' or event.key=='b'):
        grd_pos_tree.set_state("SYSTEM_TYPE,LOSS")
    elif (event.key==' '):
        grd_pos_tree.set_state("SYSTEM_TYPE,ADJUST")
    elif (event.key=='enter'):
        do_once()
    fig.canvas.draw()

def create_plt():
    # after func, new global var
    global fig,grd_ax,sim_ax

    fig, axes = plt.subplots(1,2
        ,figsize=(plt_size*2,plt_size))

    grd_ax = axes[0]
    sim_ax = axes[1]

    grd_pos_tree.set_plt_ax(grd_ax)
    sim_pos_tree.set_plt_ax(sim_ax)

    for ax in (grd_ax,sim_ax):
        ax.set_xlabel("x")
        ax.set_ylabel("y")
        ax.set_aspect('equal')
        if full_map:
            ax.set_xlim([0, pc.E])
            ax.set_ylim([0, pc.E])
        else:
            ax.set_xlim([partx, partx+size])
            ax.set_ylim([party, party+size])


    grd_ax.set_title("Grd") 
    sim_ax.set_title('Sim')
    PositionCandTree.make_grd_sim_pair(grd_pos_tree,sim_pos_tree)

    if full_map:
        lx,rx = 0+50,pc.E-50
        dy,uy = 0+50,pc.E-50
    else:
        lx,rx = partx+10,partx+size-10
        dy,uy = party+10,party+size-10

    global pos_txt,act_txt,sys_txt
    pos_txt = grd_ax.text(fontsize=font_size,x=lx,y=uy,s="Position Info",
        verticalalignment='top',horizontalalignment='left')
    act_txt = grd_ax.text(fontsize=font_size,x=rx,y=uy,s="Action Info",
        verticalalignment='top',horizontalalignment='right')
    sys_txt = grd_ax.text(fontsize=font_size,x=rx,y=dy,s="System Info",
        verticalalignment='bottom',horizontalalignment='right')

    fig.canvas.mpl_connect('key_press_event', on_press)
    
    plt.show()

def main():
    create_plt()
    # test_single_data()

if __name__ == '__main__':
    main()